//#include <ace/config.h>
#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <ctime>
#include <vector>
#include <fstream>
#include <sstream>
#include <string>
#include <vector>

#include <alcommon/alproxy.h>

#include <yarp/sig/all.h>
#include <yarp/os/all.h>
#include <yarp/dev/all.h>

#include <cv.h>
#include <cvaux.h>
#include <highgui.h>

using namespace std;

using namespace yarp::dev;
using namespace yarp::os;
using namespace yarp::sig;
using namespace yarp::sig::draw;
using namespace yarp::sig::file;

int main (int argc, char *argv [])
{
   int i;
   int count;
   int c;
  
   ostringstream imageNameLeft, imageNameRight;
  
   vector<ImageOf<PixelRgb> > imagesBuffLeft, imagesBuffRight;
  
   BufferedPort<ImageOf<PixelRgb> > imagePortLeft;
   BufferedPort<ImageOf<PixelRgb> > imagePortRight;
  
   srand(time(NULL));
   // connect to the head
   /*Network::init();
  
   imagePortLeft.open ("/image/left/in"); 
   imagePortRight.open ("/image/right/in"); 

   Network::connect ("/icub/cam/left", "/image/left/in");
   Network::connect ("/icub/cam/right", "/image/right/in");*/
   
   if(argc != 2)
   {
      std::cerr << "Wrong number of arguments!" << std::endl;
      std::cerr << "Usage: helloworld NAO_IP" << std::endl;
      exit(2);
   }

   cvNamedWindow ("Egidio bell'uomo", CV_WINDOW_AUTOSIZE);
  
   cout << "Start acquiring: " << endl;
   
   try
   {
      AL::ALProxy proxy ("ALMotion", argv[1], 9559);
      
      c = 'z';
      count = 0;
      while (c != 'q')
      {
         if (c == 'a')
         {
            //imagesBuffLeft.push_back (*(imagePortLeft.read ()));
            //imagesBuffRight.push_back (*(imagePortRight.read ()));
            //cout << "images number " << ++count << endl; 

            proxy.callVoid("walkTo", 0.07, 0.00, 0.0);
            
            cout << "walk is starting" << endl;
            while (proxy.call<bool>("walkIsActive"));
            cout << "walk ended" << endl;

            //imagesBuffLeft.push_back (*(imagePortLeft.read ()));
            //imagesBuffRight.push_back (*(imagePortRight.read ()));
            //cout << "images number " << ++count << endl;
         }
         c = cvWaitKey ();
      }
   }
   catch (const AL::ALError& e)
   {
      std::cerr << "Caught exception: " << e.what() << std::endl;
      exit(1);
   }
  
   cout << "End acquiring." << endl;
  
   cout << "Start saving" << endl;

   //for (i = 0; i < imagesBuffLeft.size (); i++)
   //{ 
   //   imageNameLeft << "C:\Users\Nigno\Documents\Datasets\Left" << i << ".ppm";
   //   yarp::sig::file::write (imagesBuffLeft [i], imageNameLeft.str ().c_str ());
   //   imageNameLeft.seekp (0);
   // 
   //   imageNameRight << "C:\Users\Nigno\Documents\Datasets\Right" << i << ".ppm";
   //   yarp::sig::file::write (imagesBuffRight [i], imageNameRight.str ().c_str ());
   //   imageNameRight.seekp (0);
   //}
  
   cout << "End saving" << endl;
  
   cvDestroyAllWindows ();
  
   return 0;
}
